#include "PoseGrabber.h"
#include"Config.h"
#include <fstream>
#include <cmath>
#include "LogCat.h"
#include <Eigen/Eigen>
#include "MyMath.h"

vector<Pose3D>& PoseGrabber::getGroudTruth()
{
	return ground_truth;
}

void PoseGrabber::getGroudTruthWithNoisy(vector<Pose3D>& GT_noisy, float noisy)
{
	MyMath mMyMath;
	for (int i = 0; i < GT_noisy.size(); i++) {
		mMyMath.poseDisturbance(GT_noisy[i], noisy);
		GT_noisy[i].fromROT4();
	}
}

bool PoseGrabber::PoseDataInit(const string & gound_truth_path)
{

	readGroudTruthOffLine(gound_truth_path);
	if(ground_truth.size()<0)
		return false;
	setPoseAsPointCloud();
	if (!mTruth_pose_cloud)
		return false;
	
	return true;
}

void PoseGrabber::readGroudTruthOffLine(const string& gound_truth_path)
{
	ifstream FileIn(gound_truth_path);
	Eigen::Matrix4d gt_data;
	Eigen::Matrix4d velo2cam, cam2velo;
	Pose3D pose;
	cam2velo << 0, 0, 1, 0,
		-1, 0, 0, 0,
		0, -1, 0, 0.08,
		0, 0, 0, 1;
	velo2cam << 0, -1, 0, 0,
		0, 0, -1, 0,
		1, 0, 0, -0.08,
		0, 0, 0, 1;

	/*
	** if you just want to get the pose_accordingto_velo transfroming from pose_cam
	**	- Camera:   x: right,		   y: down,			z: forward
	**	- Velodyne: velo_x: forward,   velo_y: left,    velo_z: up
	** then read the pose of cam ,and make
	** velo_x = z + 0.27  ;   velo_y = -x ;	velo_z = -y - 0.08;
	** if you need the whole transform , use the code bellow
	*/

	while (FileIn >> gt_data(0, 0) >> gt_data(0, 1) >> gt_data(0, 2) >> gt_data(0, 3)
		>> gt_data(1, 0) >> gt_data(1, 1) >> gt_data(1, 2) >> gt_data(1, 3)
		>> gt_data(2, 0) >> gt_data(2, 1) >> gt_data(2, 2) >> gt_data(2, 3))
	{
		gt_data(3, 0) = 0;
		gt_data(3, 1) = 0;
		gt_data(3, 2) = 0;
		gt_data(3, 3) = 1;
		gt_data = cam2velo * gt_data * velo2cam;

		//generate the yaw pitch roll /rad
		if (gt_data(2, 0) < +1) {
			if (gt_data(2, 0) > -1) {
				pose.pitch = asin(-gt_data(2, 0));
				pose.yaw = atan2(gt_data(1, 0), gt_data(0, 0));
				pose.roll = atan2(gt_data(2, 1), gt_data(2, 2));
			}
			else {
				pose.pitch = M_PI / 2;
				pose.yaw = -atan2(gt_data(1, 2), gt_data(1, 1));
				pose.roll = 0;
			}
		}
		else {
			pose.pitch = -M_PI / 2;
			pose.yaw = atan2(-gt_data(1, 2), gt_data(1, 1));
			pose.roll = 0;
		}
		pose.x = gt_data(0, 3);
		pose.y = gt_data(1, 3);
		pose.z = gt_data(2, 3);
		//save the pose to the ground_truth
		//pose.fromROT4();
		ground_truth.push_back(pose);
		LogCat::h("PoseGrabber","getGroudTruthOffLine",pose);
		//save the transfromMatrix
		transfromMatrix.push_back(gt_data);
	}
	FileIn.close();
}


void PoseGrabber::get(const int index)
{

}

void PoseGrabber::setPoseAsPointCloud()
{
	int threshold = 40;
	pcl::PointXYZ truth_point;
	for (size_t i = 0; i < ground_truth.size() - threshold; i++) {
		truth_point.x = ground_truth[i].x;
		truth_point.y = ground_truth[i].y;
		truth_point.z = ground_truth[i].z;
		mTruth_pose_cloud->points.push_back(truth_point);
	}
}

pcl::PointCloud<pcl::PointXYZ>::Ptr PoseGrabber::getPoseAsPointCloud()
{
	return mTruth_pose_cloud;
}



void PoseGrabber::clear()
{
}

PoseGrabber::PoseGrabber()
{
}


PoseGrabber::~PoseGrabber()
{
}
